#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@file motor_driver.py
@brief 电机驱动层
@details 提供电机硬件控制接口，基于Modbus驱动实现电机操作
@author Motor Controller Team
@date 2024
"""

import logging
from typing import Optional, Dict, Any
from enum import Enum

from drivers.modbus_driver import ModbusDriver
from core.constants import RegisterAddress, ControlCommands, MotorDirection
from core.logging_config import get_logger

logger = get_logger(__name__)


class MotorState(Enum):
    """电机状态枚举"""
    STOPPED = "stopped"
    RUNNING = "running"
    FAULT = "fault"
    UNKNOWN = "unknown"


class MotorDriver:
    """
    @brief 电机驱动类
    @details 负责电机硬件控制，基于Modbus驱动实现
    """
    
    def __init__(self, modbus_driver: ModbusDriver):
        """
        @brief 初始化电机驱动
        @param modbus_driver Modbus驱动实例
        """
        self.modbus_driver = modbus_driver
        self.left_motor_state = MotorState.UNKNOWN
        self.right_motor_state = MotorState.UNKNOWN
        
        logger.info("电机驱动初始化完成")
    
    def set_motor_speed(self, left_speed: Optional[int] = None, 
                       right_speed: Optional[int] = None) -> bool:
        """
        @brief 设置电机转速
        @param left_speed 左电机转速，None表示不设置
        @param right_speed 右电机转速，None表示不设置
        @return bool 设置是否成功
        """
        success = True
        
        try:
            # 设置左电机转速
            if left_speed is not None:
                if not self._validate_speed(left_speed):
                    logger.error(f"左电机转速超出范围: {left_speed}")
                    success = False
                else:
                    direction = MotorDirection.FORWARD if left_speed >= 0 else MotorDirection.REVERSE
                    speed = abs(left_speed)
                    result = self.modbus_driver.write_registers(
                        RegisterAddress.LEFT_MOTOR_SPEED, 
                        [direction.value, speed]
                    )
                    if result:
                        logger.info(f"左电机转速设置成功: {left_speed}")
                        self.left_motor_state = MotorState.RUNNING if speed > 0 else MotorState.STOPPED
                    else:
                        logger.error(f"左电机转速设置失败: {left_speed}")
                        success = False
            
            # 设置右电机转速
            if right_speed is not None:
                if not self._validate_speed(right_speed):
                    logger.error(f"右电机转速超出范围: {right_speed}")
                    success = False
                else:
                    direction = MotorDirection.FORWARD if right_speed >= 0 else MotorDirection.REVERSE
                    speed = abs(right_speed)
                    result = self.modbus_driver.write_registers(
                        RegisterAddress.RIGHT_MOTOR_SPEED, 
                        [direction.value, speed]
                    )
                    if result:
                        logger.info(f"右电机转速设置成功: {right_speed}")
                        self.right_motor_state = MotorState.RUNNING if speed > 0 else MotorState.STOPPED
                    else:
                        logger.error(f"右电机转速设置失败: {right_speed}")
                        success = False
            
            return success
            
        except Exception as e:
            logger.error(f"设置电机转速异常: {e}")
            return False
    
    def start_motors(self, left_speed: int = 500, right_speed: int = 500) -> bool:
        """
        @brief 启动电机
        @param left_speed 左电机转速
        @param right_speed 右电机转速
        @return bool 启动是否成功
        """
        logger.info(f"启动电机 - 左电机: {left_speed}, 右电机: {right_speed}")
        return self.set_motor_speed(left_speed, right_speed)
    
    def stop_motors(self) -> bool:
        """
        @brief 停止电机
        @return bool 停止是否成功
        """
        logger.info("停止电机")
        success = self.set_motor_speed(0, 0)
        if success:
            self.left_motor_state = MotorState.STOPPED
            self.right_motor_state = MotorState.STOPPED
        return success
    
    def emergency_stop(self) -> bool:
        """
        @brief 急停电机
        @return bool 急停是否成功
        """
        logger.warning("执行急停操作")
        
        try:
            # 发送急停命令
            success = self.modbus_driver.write_register(
                RegisterAddress.SYSTEM_CONTROL, 
                ControlCommands.EMERGENCY_STOP
            )
            
            if success:
                self.left_motor_state = MotorState.STOPPED
                self.right_motor_state = MotorState.STOPPED
                logger.warning("急停执行成功")
            else:
                logger.error("急停执行失败")
            
            return success
            
        except Exception as e:
            logger.error(f"急停异常: {e}")
            return False
    
    def phase_learning(self) -> bool:
        """
        @brief 执行相序学习
        @return bool 相序学习是否成功
        """
        logger.info("开始执行相序学习")
        
        try:
            success = self.modbus_driver.write_register(
                RegisterAddress.SYSTEM_CONTROL, 
                ControlCommands.PHASE_LEARNING
            )
            
            if success:
                logger.info("相序学习命令发送成功")
            else:
                logger.error("相序学习失败")
            
            return success
            
        except Exception as e:
            logger.error(f"相序学习异常: {e}")
            return False
    
    def reset_system(self) -> bool:
        """
        @brief 复位系统
        @return bool 复位是否成功
        """
        logger.info("执行系统复位")
        
        try:
            success = self.modbus_driver.write_register(
                RegisterAddress.SYSTEM_CONTROL, 
                ControlCommands.RESET
            )
            
            if success:
                self.left_motor_state = MotorState.UNKNOWN
                self.right_motor_state = MotorState.UNKNOWN
                logger.info("系统复位成功")
            else:
                logger.error("系统复位失败")
            
            return success
            
        except Exception as e:
            logger.error(f"系统复位异常: {e}")
            return False
    
    def get_motor_status(self) -> Dict[str, Any]:
        """
        @brief 获取电机状态
        @return Dict[str, Any] 电机状态信息
        """
        status = {
            'left_motor_state': self.left_motor_state.value,
            'right_motor_state': self.right_motor_state.value,
            'timestamp': None
        }
        
        try:
            import time
            status['timestamp'] = time.time()
            
            # 读取电机状态寄存器
            left_status = self.modbus_driver.read_register(RegisterAddress.LEFT_MOTOR_STATUS)
            right_status = self.modbus_driver.read_register(RegisterAddress.RIGHT_MOTOR_STATUS)
            system_status = self.modbus_driver.read_register(RegisterAddress.SYSTEM_STATUS)
            error_code = self.modbus_driver.read_register(RegisterAddress.ERROR_CODE)
            
            if left_status is not None:
                status['left_motor_register_status'] = left_status
            if right_status is not None:
                status['right_motor_register_status'] = right_status
            if system_status is not None:
                status['system_register_status'] = system_status
            if error_code is not None:
                status['error_code'] = error_code
                
        except Exception as e:
            logger.error(f"读取电机状态异常: {e}")
        
        return status
    
    def _validate_speed(self, speed: int) -> bool:
        """
        @brief 验证转速是否在有效范围内
        @param speed 转速值
        @return bool 转速是否有效
        """
        return -1000 <= speed <= 1000
    
    def get_motor_states(self) -> Dict[str, str]:
        """
        @brief 获取电机状态
        @return Dict[str, str] 电机状态字典
        """
        return {
            'left_motor': self.left_motor_state.value,
            'right_motor': self.right_motor_state.value
        }
